#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/common/common.h>
#include <iostream>
#include <thread>
#include <chrono>

typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;
typedef pcl::PointNormal PointNormalT;
typedef pcl::PointCloud<PointNormalT> PointCloudNormalT;

int main(int argc, char** argv) {
    std::cout << "Starting multi-point cloud visualization..." << std::endl;

    // 创建PCL可视化器
    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("Multi Point Cloud Viewer"));
    viewer->setBackgroundColor(0, 0, 0);
    viewer->addCoordinateSystem(20.0);
    viewer->initCameraParameters();

    // 定义要加载的点云文件路径（使用build目录下的实际文件）
    std::vector<std::string> cloud_files = {
        "_filtered.pcd",
        "_gluing_path.pcd",
        "_gluing_path_with_normals.pcd",
        "_inner_gluing_points.pcd"
    };

    // 定义颜色
    std::vector<std::array<int, 3>> colors = {
        {128, 128, 128},  // 灰色 - 滤波后的原始点云
        {255, 0, 0},      // 红色 - 打胶路径点云
        {0, 255, 0},      // 绿色 - 带法向量的路径点云
        {0, 0, 255}       // 蓝色 - 内部胶合点云
    };

    // 定义点云名称
    std::vector<std::string> cloud_names = {
        "filtered_cloud",
        "gluing_path_cloud",
        "gluing_path_normals_cloud",
        "inner_gluing_points_cloud"
    };

    // 定义点云显示名称
    std::vector<std::string> display_names = {
        "Filtered Point Cloud",
        "Gluing Path", 
        "Gluing Path with Normals",
        "Inner Gluing Points"
    };

    int loaded_clouds = 0;

    // 加载并显示所有点云
    for (int i = 0; i < 4; ++i) {
        if (i == 2) {
            // 特殊处理带法向量的点云
            PointCloudNormalT::Ptr normal_cloud(new PointCloudNormalT);
            
            if (pcl::io::loadPCDFile<PointNormalT>(cloud_files[i], *normal_cloud) == -1) {
                std::cerr << "Warning: Couldn't read file " << cloud_files[i] << std::endl;
                continue;
            }

            std::cout << "Loaded " << normal_cloud->size() << " points with normals from " << cloud_files[i] << std::endl;

            // 添加带法向量的点云到可视化器
            pcl::visualization::PointCloudColorHandlerCustom<PointNormalT> normal_color_handler(
                normal_cloud, colors[i][0], colors[i][1], colors[i][2]);
            viewer->addPointCloud<PointNormalT>(normal_cloud, normal_color_handler, cloud_names[i]);
            
            // 添加法向量箭头显示
            viewer->addPointCloudNormals<PointNormalT>(normal_cloud, 1, 5.0, cloud_names[i] + "_normals");
            
            // 设置点大小
            viewer->setPointCloudRenderingProperties(
                pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 10, cloud_names[i]);
        } else {
            // 处理普通点云
            PointCloudT::Ptr cloud(new PointCloudT);
            
            if (pcl::io::loadPCDFile<PointT>(cloud_files[i], *cloud) == -1) {
                std::cerr << "Warning: Couldn't read file " << cloud_files[i] << std::endl;
                continue;
            }

            std::cout << "Loaded " << cloud->size() << " points from " << cloud_files[i] << std::endl;

            // 添加点云到可视化器
            pcl::visualization::PointCloudColorHandlerCustom<PointT> color_handler(
                cloud, colors[i][0], colors[i][1], colors[i][2]);
            viewer->addPointCloud<PointT>(cloud, color_handler, cloud_names[i]);
            
            // 设置点大小 - 路径点云放大方便观察
            int point_size;
            if (i == 0) {
                point_size = 3;  // 滤波后的点云较小
            } else if (i == 3) {
                point_size = 12; // 内部胶合点云最大，方便观察
            } else {
                point_size = 8;  // 路径点云放大方便观察
            }
            viewer->setPointCloudRenderingProperties(
                pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, cloud_names[i]);
        }

        loaded_clouds++;
    }

    if (loaded_clouds == 0) {
        std::cerr << "Error: No point clouds could be loaded!" << std::endl;
        return -1;
    }

    std::cout << "\nLoaded " << loaded_clouds << " point clouds successfully!" << std::endl;
    
    // 设置相机视角
    viewer->setCameraPosition(0, 0, 100, 0, 0, 0, 0, 1, 0);

    // 添加文本说明
    std::string info_text = "Gluing Path Point Clouds:\n";
    for (int i = 0; i < static_cast<int>(display_names.size()) && i < loaded_clouds; ++i) {
        info_text += "- " + display_names[i] + "\n";
    }
    info_text += "\nControls:\n";
    info_text += "- Mouse: Rotate view\n";
    info_text += "- Mouse wheel: Zoom\n";
    info_text += "- Right click + drag: Pan\n";
    info_text += "- Press 'q' to quit\n";
    info_text += "- Press '1'-'4' to toggle clouds";

    viewer->addText(info_text, 10, 10, 12, 1.0, 1.0, 1.0, "info_text");

    // 键盘回调函数
    std::cout << "\nVisualization Controls:" << std::endl;
    std::cout << "- Mouse: Rotate view" << std::endl;
    std::cout << "- Mouse wheel: Zoom in/out" << std::endl;
    std::cout << "- Right click + drag: Pan view" << std::endl;
    std::cout << "- Press '1' to toggle filtered point cloud" << std::endl;
    std::cout << "- Press '2' to toggle gluing path" << std::endl;
    std::cout << "- Press '3' to toggle gluing path with normals" << std::endl;
    std::cout << "- Press '4' to toggle inner gluing points" << std::endl;
    std::cout << "- Press 'q' to quit" << std::endl;

    // 显示循环
    while (!viewer->wasStopped()) {
        viewer->spinOnce(60000);
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }

    std::cout << "Visualization ended." << std::endl;
    return 0;
}
